# Copyright 2020 Makani Technologies LLC
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#      http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""Wing IMU calibration."""

from makani.config import mconfig
from makani.control import system_types


@mconfig.Config(deps={
    'wing_serial': 'common.wing_serial',
})
def MakeParams(params):
  # Magnetometer calibrations are generated by
  # analysis/util/calibrate_imu_channel.m applied to logs from the Hokey
  # Pokey. See go/makani-hokey-pokey.
  if params['wing_serial'] == system_types.kWingSerial01:
    mag_cal_a = [
        {'scale': 0.991308, 'bias': 0.0129853, 'bias_count': 0},
        {'scale': 0.992628, 'bias': 0.0158393, 'bias_count': 0},
        {'scale': 1.00202, 'bias': -0.011001, 'bias_count': 0},
    ]
    mag_cal_b = [
        {'scale': 0.992772, 'bias': -0.0133809, 'bias_count': 0},
        {'scale': 0.994943, 'bias': 0.05325, 'bias_count': 0},
        {'scale': 1.00363, 'bias': -0.0258979, 'bias_count': 0},
    ]
    mag_cal_c = [
        {'scale': 0.992142, 'bias': -0.0111141, 'bias_count': 0},
        {'scale': 0.994469, 'bias': -0.0101217, 'bias_count': 0},
        {'scale': 1.00098, 'bias': -0.0281789, 'bias_count': 0},
    ]

  # TODO: OktKite. Migrate OktoberKite values into its own config when
  # hardware is available.
  elif params['wing_serial'] in (system_types.kWingSerial04Hover,
                                 system_types.kWingSerial04Crosswind,
                                 system_types.kWingSerialOktoberKite01):
    # Calibrated using 20190114-113529-YM600-04_hokey-pokey_fcu-B_170329-710-
    # 01224-02-MKANI-01417_fcu-A_170329-710-01223-02-MKANI-01416_in_pasture_
    # perplexing_cattle.h5
    mag_cal_a = [
        {'scale': 0.998574, 'bias': -0.0451766, 'bias_count': 0},
        {'scale': 1.00474, 'bias': -0.0020454, 'bias_count': 0},
        {'scale': 1.01218, 'bias': -0.0467952, 'bias_count': 0},
    ]
    mag_cal_b = [
        {'scale': 1.00122, 'bias': -0.0154187, 'bias_count': 0},
        {'scale': 0.998088, 'bias': 0.0306831, 'bias_count': 0},
        {'scale': 1.0118, 'bias': -0.0317695, 'bias_count': 0},
    ]
    mag_cal_c = [
        {'scale': 1.00861, 'bias': 0.0257766, 'bias_count': 0},
        {'scale': 0.997336, 'bias': 0.00251033, 'bias_count': 0},
        {'scale': 1.00911, 'bias': -0.0177624, 'bias_count': 0},
    ]
  elif params['wing_serial'] in (system_types.kWingSerial05Hover,
                                 system_types.kWingSerial05Crosswind):
    # Hokey-pokey data from YM600-05_20190718-095717-hokey_pokey_2.h5
    mag_cal_a = [
        {'scale': 0.990106, 'bias': -0.0540549, 'bias_count': 0},
        {'scale': 0.995874, 'bias': -0.00644475, 'bias_count': 0},
        {'scale': 1.00012, 'bias': -0.0797074, 'bias_count': 0},
    ]
    mag_cal_b = [
        {'scale': 0.994941, 'bias': -0.0465519, 'bias_count': 0},
        {'scale': 0.996176, 'bias': -0.00101688, 'bias_count': 0},
        {'scale': 1.00845, 'bias': -0.053553, 'bias_count': 0},
    ]
    mag_cal_c = [
        {'scale': 0.998271, 'bias': 0.043658, 'bias_count': 0},
        {'scale': 0.99416, 'bias': 0.00251655, 'bias_count': 0},
        {'scale': 1.00245, 'bias': -0.0456833, 'bias_count': 0},
    ]
  elif params['wing_serial'] in (system_types.kWingSerial06Hover,
                                 system_types.kWingSerial06Crosswind):
    # TODO(b/122921648): The following data must be measured by doing
    # the hokey-pokey.
    mag_cal_a = [
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
    ]
    mag_cal_b = [
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
    ]
    mag_cal_c = [
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
    ]
  elif params['wing_serial'] in (system_types.kWingSerial07Hover,
                                 system_types.kWingSerial07Crosswind):
    # TODO(b/145244768): The following data must be measured by doing
    # the hokey-pokey.
    mag_cal_a = [
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
    ]
    mag_cal_b = [
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
    ]
    mag_cal_c = [
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
        {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
    ]
  else:
    assert False, 'No magnetometer calibration available.'

  # The IMU positions and orientations are based on measurements
  # provided by criner, 2016-07-29, https://goo.gl/dySvM5.
  #
  # For identification purposes, note:
  #
  # * IMUs A and C are both located in FCU A.
  #
  # * IMU A and the Septentrio GPS are connected to Flight Computer A,
  #   as may be confirmed via network.yaml.
  return [
      # IMU A.
      {
          # Parent coordinate system.
          'parent_cs': system_types.kCoordinateSystemBody,

          # Position [m] of the IMU in parent (body) coordinates.
          'pos': [0.390, 0.085, 0.534],

          # Direction cosine matrix that takes parent (body) coordinates to IMU
          # coordinates.
          'dcm_parent2m': {'d': [[1.0, 0.0, 0.0],
                                 [0.0, -1.0, 0.0],
                                 [0.0, 0.0, -1.0]]},

          # Calibration parameters (e.g. scales and biases) for the IMU
          # measurements.
          'acc_cal': [
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0}
          ],
          'gyro_cal': [
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0}
          ],
          'mag_cal': mag_cal_a,
          'pressure_cal': {'scale': 1e5, 'bias': 0.0, 'bias_count': 0}
      },

      # IMU B.
      {
          # Parent coordinate system.
          'parent_cs': system_types.kCoordinateSystemBody,

          # Position [m] of the IMU in parent (body) coordinates.
          'pos': [0.390, -0.085, 0.438],

          # Direction cosine matrix that takes parent (body) coordinates to IMU
          # coordinates.
          'dcm_parent2m': {'d': [[1.0, 0.0, 0.0],
                                 [0.0, 1.0, 0.0],
                                 [0.0, 0.0, 1.0]]},

          # Calibration parameters (e.g. scales and biases) for the IMU
          # measurements.
          'acc_cal': [
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0}
          ],
          'gyro_cal': [
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0}
          ],
          'mag_cal': mag_cal_b,
          'pressure_cal': {'scale': 1e5, 'bias': 0.0, 'bias_count': 0}
      },

      # IMU C.
      {
          # Parent coordinate system.
          'parent_cs': system_types.kCoordinateSystemBody,

          # Position [m] of the IMU in parent (body) coordinates.
          'pos': [0.390, -0.042, 0.534],

          # Direction cosine matrix that takes parent (body) coordinates to IMU
          # coordinates.
          'dcm_parent2m': {'d': [[1.0, 0.0, 0.0],
                                 [0.0, -1.0, 0.0],
                                 [0.0, 0.0, -1.0]]},

          # Calibration parameters (e.g. scales and biases) for the IMU
          # measurements.
          'acc_cal': [
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0}
          ],
          'gyro_cal': [
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0},
              {'scale': 1.0, 'bias': 0.0, 'bias_count': 0}
          ],
          'mag_cal': mag_cal_c,
          'pressure_cal': {'scale': 1e5, 'bias': 0.0, 'bias_count': 0}
      }
  ]
